#pragma once
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "iostream"
#include "v5.h"
#include "v5_vcs.h"
using namespace vex;
using namespace std;
using signature = vex::vision::signature;

// ROBOT的定义在IO_Setup_All.cpp中
#define R_74000 0
#define R_74000M 11
#define R_74000V 12
#define R_74000P 13
#define R_74000S 14
#define R_74000Y 15
#define R_70181A 21
#define R_70181B 22
#define R_263R 31
#define R_263Z 32
#define R_263T 33
#define R_7792 100
#define R_1306A 36

extern const int ROBOT;           // defined in ___IO_ALL.cpp
extern const string IO_FILE_NAME; // defined in ___IO_ALL.cpp
extern const string team_name;    // defined in IO_setup_xxx.h

// 阻塞与非阻塞定义
enum class Block {
  yes = true,
  no = false,
  foreground = yes,
  background = no,
};

#define PI 3.141592653

// 系统全局变量声明
extern vex::brain Brain;
extern vex::competition Competition;

extern color screen_brainPenColor; // 全局画笔颜色
void screen_set_pen_color(color c);
color screen_get_pen_color();
void screen_restore_pen_color();       // 保存画笔颜色
void screen_store_pen_color();         // 恢复画笔颜色
void screen_set_pen_font(fontType ft); // 设置字体

#define Print Brain.Screen.printAt
#define special_key ctrl.ButtonLeft

void wait(int milsec);

#define WHILE(condition)                                                       \
  do {                                                                         \
  } while (condition)

#define while_wait(condition)                                                  \
  do {                                                                         \
    wait(1);                                                                   \
  } while (condition)

#define while_wait_timeout(condition, timeout)                                 \
  {                                                                            \
    timer _t;                                                                  \
    do {                                                                       \
      wait(1);                                                                 \
    } while (condition && _t.time() < timeout);                                \
  }

#define repeat(count) for (int n = 0; n < count; n++)

#define waitUntil(condition)                                                   \
  do {                                                                         \
    wait(5, msec);                                                             \
  } while (!(condition))

void pre_auton(void);
void _autonomous(void);
void _usercontrol(void);

// 常用数学函数
#define abs(x) (((x) > 0) ? (x) : -(x))
int sgn(double x);
void zero_out_blind(double &x, double threshold);
void abs_max_limit(double &x, double max_bnd);
void abs_min_limit(double &x, double min_bnd);
void value_boundary(double &x, double min, double max);

constexpr double k_deg_to_rad = 3.141592653 / 180.0;
constexpr double k_rad_to_deg = 180.0 / 3.141592653;
#define deg_to_rad(agl) (agl * k_deg_to_rad)
#define rad_to_deg(rad) (rad * k_rad_to_deg)
double get_displacement_pct(double dst_now, double begin_dst, double aim_dst);

// 视觉标签色
extern vision::signature vis_BLUE;
extern vision::signature vis_RED;
extern vision::signature vis_GREEN;
extern vision::signature vis_YELLOW;
//
/////////////////////////////////////////////////////////////
// 标准电磁阀：掉电后状态为false
/*
  电磁阀控制线定义,可以使用标准电磁阀控制器，或者V4马达控制器
  使用马达控制器时，接线也是马达控制器的两线左黑右红地插入电磁阀的插口)
  执行set(true)后，程序会有反应时间，马达控制器需要160ms左右才能输出高电平,
  电磁阀控制器则需要120ms左右才能输出高电平

  *开机但没有运行程序（或退出程序后），端口会输出高电平，此时电磁阀控制器会输出高电平，马达控制器则会输出低电平
  *刚进入程序时，初始化接口前，端口会输出短暂瞬间的低电平，此时电磁阀控制器和马达控制器均会输出低电平
*/
class Solenoid_Driver {
public:
  enum Solenoid_Driver_Type {
    Motor29,
    DigitalOut,
  };

private:
  bool use_motor29;
  digital_out *dgt_o;
  motor29 *mtr_ctrl;
  vex::triport::port *port0;
  bool state;
  string pne_name = "pne";

public:
  char get_port_char() { return (port0 - &Brain.ThreeWirePort.A) + 'A'; }
  string name() { return pne_name.c_str(); }

public:
  Solenoid_Driver(string name, vex::triport::port &port,
                  Solenoid_Driver_Type type, bool default_status = false) {
    port0 = &port;
    if (type == Motor29) {
      use_motor29 = true;
      mtr_ctrl = new vex::motor29(port, true);
    } else {
      use_motor29 = false;
      dgt_o = new vex::digital_out(port);
    }
    state = default_status;
    pne_name = name;
    set(default_status); // 默认输出低电平
  }

  void set(bool value) {
    state = value;
    if (use_motor29) {
      if (value) { // 输出高电平
        mtr_ctrl->setVelocity(100, percent);
        mtr_ctrl->spin(vex::forward);
      } else { // 输出稳定的微弱反向电平，用来模拟低电平
        mtr_ctrl->setVelocity(-1, percent);
        mtr_ctrl->spin(vex::forward);
        // mtr_ctrl->stop();//stop会输出不稳定的变化电压
      }
    } else {
      dgt_o->set(value);
    }
  }
  bool get_state() { return state; }
  void toggle_state() { set(!state); }
};
// 使用示例：
/* Solenoid_Driver pne_occupying_lock =
     Solenoid_Driver("pne_lock", Brain.ThreeWirePort.B,
                     Solenoid_Driver::Solenoid_Driver_Type::DigitalOut);
*/

extern long gt;
#define timing_us_start() gt = timer::systemHighResolution();
#define get_timing_us() (timer::systemHighResolution() - gt)
#define cout_timing_us(hint)                                                   \
  cout << hint << timer::systemHighResolution() - gt << endl;
